
/*
 *
 * Copyright (c) 2006 P.J.Leonard
 * 
 * http://www.frinika.com
 * 
 * This file is part of Frinika.
 * 
 * Frinika is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.

 * Frinika is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.

 * You should have received a copy of the GNU General Public License
 * along with Frinika; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */
package sane;

import uk.ac.bath.util.MyRandom;

/**
 *
 * @author pjl
 */
public class CartSim {

    float one_degree = 0.0174532f;	/* 2pi/360 */

    float six_degrees = 0.1047192f;
    float twelve_degrees = 0.2094384f;
    float fifty_degrees = 0.87266f;

    int run(Network net, int max_steps, boolean RANDOM_START) {
        float x, /* cart position, meters */
                x_dot, /* cart velocity */
                theta, /* pole angle, radians */
                theta_dot;		/* pole angular velocity */
        int steps = 0, y;

        if (Context.debugNet)    System.out.println("---------------------------------------------------------");
        if (RANDOM_START) {
            /*set up random start state*/
            x =  (float) (0.5*(MyRandom.nextInt(4800)/ 1000.0 - 2.4));
            x_dot =(float)  (0.5*(MyRandom.nextInt(2000) / 1000.0 - 1.0));
            theta = (float) (0.5*(MyRandom.nextInt(400) / 1000.0 - .2));
            theta_dot = (float) (0.5*(MyRandom.nextInt(3000) / 1000.0 - 1.5));
        } else {
            x = x_dot = theta = theta_dot = 0.0f;
        }

        
        CartPole cp = new CartPole(x,x_dot,theta,theta_dot);

        /*--- Iterate through the action-learn loop. ---*/
        while (steps++ < max_steps) {

            /*-- setup the input layer based on the four iputs --*/
            net.setIn(0, cp.x);
            net.setIn(1, cp.x_dot);
            net.setIn(2, cp.theta);
            net.setIn(3, cp.theta_dot);
            net.setIn(4,  0.5f);
            net.tick();   /*-- activate the network based on the input --*/

            /*-- decide which way to push via which output unit is greater --*/
            if (net.out(0) > net.out(1)) {
                y = -1;
            } else {
                y = 1;
            }

            /*--- Apply action to the simulated cart-pole ---*/
    //        System.out.println(y+ " | " + cp );

            cp.tick(y);

            /*--- Check for failure.  If so, return steps ---*/
            if (cp.x < -2.4 || cp.x > 2.4 || cp.theta < -twelve_degrees ||
                    cp.theta > twelve_degrees) {
                return steps;
            }
        }
        return steps;
    }

   
}
